/*
 * mod_filter.c
 *
 *  Created on: 2025 Feb 18
 *      Author: SFLY
 */
#include "mod_filter.h"
#include "mod_math.h"



/**
 *******************************************************************************
 ** \brief: initialize first order low pass filter
 **
 ** \param [in] stc_1stLpf_cfg_t stcCfg: configuration parameters
 ** \param [in] stc_1stLpf_t* pstc: pointer to low-pass filter
 **
 ** \retval     none
 ******************************************************************************/
void Filter_InitFirstLpf(float Fc,float Fs, stc_1stLpf_t *pstc){
    pstc->LpfK      =  __builtin_dsp_fsdiv_qx320f(Fc, Fs);
    pstc->Input     = 0;
    pstc->Output    = 0;
}

/**
 *******************************************************************************
 ** \brief: first order low-pass filter:
 **             y(n) = y(n-1) + Wc * Ts * [(x(n) + x(n-1)) / 2 - y(n-1)]
 **
 ** \param [in] float i32Q12_Input: input of filter
 ** \param [in] stc_1stLpf_t* pstc: pointer to low-pass filter
 **
 ** \retval     float Output: output of filter
 ******************************************************************************/
float Filter_FirstLpf(float Input, stc_1stLpf_t *pstc) {
    float temp      =   (Input + pstc->Input) * 0.5f - pstc->Output;
    pstc->Output    +=  pstc->LpfK * temp;
    pstc->Input     =   Input;
    return pstc->Output;
}

/**
 *******************************************************************************
 ** \brief: reset states of first order low-pass filter
 **
 ** \param [in] float i32Q12_Output:  set filter ouput
 ** \param [in] stc_1stLpf_t *pstc:     pointer of first order low-pass filter
 **
 ** \retval     none
 ******************************************************************************/
void Filter_RstFirstLpf(float Output, stc_1stLpf_t *pstc) {
    pstc->Input     = Output;
    pstc->Output    = Output;
}


void Filter_InitPeriodAvg(float avg, stc_period_avg_t *pstc)
{
    pstc->Avg = avg;
    pstc->Cnt = 0.0f;
    pstc->Sum = 0.0f;
    pstc->InvTotalCnt = 1000.0f / (10.0f*Motor_SYS_TICK_FREQ_HZ);
}

float Filter_PeriodAvg(float Input, stc_period_avg_t *pstc)
{
    pstc->Sum += Input;
    pstc->Cnt += pstc->InvTotalCnt;
    if(pstc->Cnt >= 1.0f) {
        pstc->Avg = pstc->Sum * pstc->InvTotalCnt;
        pstc->Sum = 0;
        pstc->Cnt = 0;
    }
    return pstc->Avg;
}
void Filter_RstPeriodAvg(float Output, stc_period_avg_t *pstc) {
    pstc->Avg = Output;
    pstc->Sum = Output;
    pstc->Cnt = 0;
}

// 初始化EKF（保持不变）
void EKF_Init(EKF_Estimator* ekf,
              float init_theta, float init_omega,
              float init_P_theta, float init_P_omega,
              float Q_theta, float Q_omega,
              float R_theta) {
    ekf->theta = init_theta;
    ekf->omega = init_omega;
    ekf->P[0][0] = init_P_theta;
    ekf->P[0][1] = 0.0f;
    ekf->P[1][0] = 0.0f;
    ekf->P[1][1] = init_P_omega;
    ekf->Q_theta = Q_theta;
    ekf->Q_omega = Q_omega;
    ekf->R_theta = R_theta;
}

// EKF预测步骤（保持不变）
void EKF_Predict(EKF_Estimator* ekf, float dt) {
    float theta_pred = ekf->theta + ekf->omega * dt;
    float omega_pred = ekf->omega;

    float dt_sq = dt * dt;
    float P00 = ekf->P[0][0];
    float P01 = ekf->P[0][1];
    float P11 = ekf->P[1][1];

    ekf->theta = theta_pred;
    ekf->omega = omega_pred;
    ekf->P[0][0] = P00 + 2 * dt * P01 + dt_sq * P11 + ekf->Q_theta;
    ekf->P[0][1] = P01 + dt * P11;
    ekf->P[1][0] = ekf->P[0][1];
    ekf->P[1][1] = P11 + ekf->Q_omega;
}

// EKF更新步骤（优化后）
void EKF_Update(EKF_Estimator* ekf, float meas_theta) {
    // 计算观测残差
    const float y = meas_theta - ekf->theta;

    // 简化创新协方差计算（H=[1,0]）
    const float S = ekf->P[0][0] + ekf->R_theta;
    if (fabsf(S) < 1e-6f) return;

    // 简化卡尔曼增益计算
    const float K0 = ekf->P[0][0] / S;
    const float K1 = ekf->P[0][1] / S;

    // 更新状态
    ekf->theta += K0 * y;
    ekf->omega += K1 * y;

    // 预计算公共系数
    const float one_minus_K0 = 1.0f - K0;
    const float P00 = ekf->P[0][0];
    const float P01 = ekf->P[0][1];
    const float P11 = ekf->P[1][1];

    // 更新协方差矩阵（显式计算所有元素）
    const float new_P00 = one_minus_K0 * P00;
    const float new_P01 = one_minus_K0 * P01;
    const float new_P11 = P11 - K1 * P01;

    ekf->P[0][0] = new_P00;
    ekf->P[0][1] = new_P01;
    ekf->P[1][0] = new_P01; // 保持对称性
    ekf->P[1][1] = new_P11;
}
